
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_vector3.c
  * @author     baiyang
  * @date       2021-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_math.h"
#include "gp_vector3.h"

#include <common/gp_defines.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/** 
  * @brief       三维向量赋值
  * @param[in]   vo  
  * @param[in]   x  
  * @param[in]   y  
  * @param[in]   z  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_set(Vector3f_t* vo, const float x, const float y, const float z)
{
    vo->x = x;
    vo->y = y;
    vo->z = z;
}

/** 
  * @brief       三维向量初始化为0
  * @param[in]   vo
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_zero(Vector3f_t* vo)
{
    vo->x = 0.0f;
    vo->y = 0.0f;
    vo->z = 0.0f;
}

/**
  * @brief       
  * @param[in]   vo  
  * @param[out]  
  * @retval      
  * @note        
  */
bool vec3_is_zero(const Vector3f_t* vo)
{
    if (math_flt_zero(vo->x) || math_flt_zero(vo->x) || math_flt_zero(vo->x)) {
        return true;
    }

    return false;
}

/** 
  * @brief       计算三维向量模值
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_length(const Vector3f_t* v)
{
    return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z); 
}

float vec3_length_xy(const Vector3f_t* v)
{
    return sqrtf(v->x * v->x + v->y * v->y); 
}

// limit xy component vector to a given length. returns true if vector was limited
bool vec3_limit_length_xy(Vector3f_t* v, float max_length)
{
    const float length_xy = sqrtf(v->x * v->x + v->y * v->y);
    if ((length_xy > max_length) && math_flt_positive(length_xy)) {
        v->x *= (max_length / length_xy);
        v->y *= (max_length / length_xy);
        return true;
    }
    return false;
}

/** 
  * @brief       计算三维向量模值的平方
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_length_squared(const Vector3f_t* v)
{
    return v->x * v->x + v->y * v->y + v->z * v->z; 
}

/** 
  * @brief       三维向量单位化
  * @param[in]   vo  
  * @param[in]   vi  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_norm(Vector3f_t* vo, const Vector3f_t* vi)
{
    float len = vec3_length(vi);
    if(math_flt_zero(len))
    {
        vec3_zero(vo);
    }
    else
    {
        vo->x = vi->x / len;
        vo->y = vi->y / len;
        vo->z = vi->z / len;
    }
}

/** 
  * @brief       三维向量加运算(vo = v1 + v2)
  * @param[in]   vo  
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_add(Vector3f_t* vo, const Vector3f_t* v1, const Vector3f_t* v2)
{
    vo->x = v1->x + v2->x;
    vo->y = v1->y + v2->y;
    vo->z = v1->z + v2->z;
}

/** 
  * @brief       三维向量加运算(vo = v1 + v2)
  * @param[in]   vo  
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_add2(Vector3f_t* vo, const Vector3f_t* v1, const Vector3f_t* v2, const Vector3f_t* v3)
{
    vo->x = v1->x + v2->x + v3->x;
    vo->y = v1->y + v2->y + v3->x;
    vo->z = v1->z + v2->z + v3->x;
}

/** 
  * @brief       三维向量减运算（vo = v1 - v2）
  * @param[in]   vo  
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_sub(Vector3f_t* vo, const Vector3f_t* v1, const Vector3f_t* v2)
{
    vo->vec3[0] = v1->vec3[0] - v2->vec3[0];
    vo->vec3[1] = v1->vec3[1] - v2->vec3[1];
    vo->vec3[2] = v1->vec3[2] - v2->vec3[2];
}

/** 
  * @brief       三维向量减运算（vo = v1 - v2）
  * @param[in]   vo  
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector3f_t vec3_sub2(const Vector3f_t* v1, const Vector3f_t* v2)
{
    Vector3f_t vo;

    vo.vec3[0] = v1->vec3[0] - v2->vec3[0];
    vo.vec3[1] = v1->vec3[1] - v2->vec3[1];
    vo.vec3[2] = v1->vec3[2] - v2->vec3[2];

    return vo;
}

/** 
  * @brief       叉乘运算
  * @param[in]   vo  
  * @param[in]   vl  
  * @param[in]   vr  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_cross(Vector3f_t* vo, const Vector3f_t* vl, const Vector3f_t* vr)
{
    Vector3f_t vl_tmp = *vl;
    Vector3f_t vr_tmp = *vr;

    vo->vec3[0] = vl_tmp.vec3[1]*vr_tmp.vec3[2] - vl_tmp.vec3[2]*vr_tmp.vec3[1];
    vo->vec3[1] = vl_tmp.vec3[2]*vr_tmp.vec3[0] - vl_tmp.vec3[0]*vr_tmp.vec3[2];
    vo->vec3[2] = vl_tmp.vec3[0]*vr_tmp.vec3[1] - vl_tmp.vec3[1]*vr_tmp.vec3[0];
}

/** 
  * @brief       点乘运算
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_dot(const Vector3f_t* v1, const Vector3f_t* v2)
{
    return v1->vec3[0] * v2->vec3[0] + v1->vec3[1] * v2->vec3[1] + v1->vec3[2] * v2->vec3[2];
}

/** 
  * @brief       运放（vo = vi * scalar）
  * @param[in]   vo  
  * @param[in]   vi  
  * @param[in]   scalar  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_mult(Vector3f_t* vo, const Vector3f_t* vi, const float scalar)
{
    vo->vec3[0] = vi->vec3[0] * scalar;
    vo->vec3[1] = vi->vec3[1] * scalar;
    vo->vec3[2] = vi->vec3[2] * scalar;
}

/** 
  * @brief       运放（vo = vi * scalar）
  * @param[in]   vo  
  * @param[in]   vi  
  * @param[in]   scalar  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector3f_t vec3_mult2(const Vector3f_t* vi, const float scalar)
{
    Vector3f_t vo;

    vo.vec3[0] = vi->vec3[0] * scalar;
    vo.vec3[1] = vi->vec3[1] * scalar;
    vo.vec3[2] = vi->vec3[2] * scalar;

    return vo;
}

// non-uniform scaling
void vec3_mult_scalar_vec(Vector3f_t* vo, const Vector3f_t* vi, const Vector3f_t* scalar)
{
    vo->x = vi->x * scalar->x;
    vo->y = vi->y * scalar->y;
    vo->z = vi->z * scalar->z;
}

/** 
  * @brief       运放（vo = vi * scalar + vi2 * scalar2）
  * @param[in]   vo  
  * @param[in]   vi  
  * @param[in]   scalar  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector3f_t vec3_mult_add(const Vector3f_t* vi, const float scalar, const Vector3f_t* vi2, const float scalar2)
{
    Vector3f_t vo;

    vo.vec3[0] = vi->vec3[0] * scalar + vi2->vec3[0] * scalar2;
    vo.vec3[1] = vi->vec3[1] * scalar + vi2->vec3[1] * scalar2;
    vo.vec3[2] = vi->vec3[2] * scalar + vi2->vec3[2] * scalar2;

    return vo;
}

/**
  * @brief       
  * @param[in]   vo  
  * @param[in]   vl  
  * @param[in]   vr  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_hadamard_product(Vector3f_t* vo, const Vector3f_t* vl, const Vector3f_t* vr)
{
    vo->vec3[0] = vl->vec3[0] * vr->vec3[0];
    vo->vec3[1] = vl->vec3[1] * vr->vec3[1];
    vo->vec3[2] = vl->vec3[2] * vr->vec3[2];
}

// return bearing in centi-degrees between two positions
float vec3_get_bearing_cd(const Vector3f_t *origin, const Vector3f_t *destination)
{
    float bearing = atan2f(destination->y-origin->y, destination->x-origin->x) * DEGX100;
    if (bearing < 0) {
        bearing += 36000.0f;
    }
    return bearing;
}

/**
  * @brief       
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_angle_xy(const Vector3f_t *v)
{
    return M_PI_2 + atan2f(-v->x, v->y);
}

void vec3_rotate(Vector3f_t *v, enum RotationEnum rotation)
{
    float tmp;
    switch (rotation) {
    case ROTATION_NONE:
        return;
    case ROTATION_YAW_45: {
        tmp = HALF_SQRT_2*(ftype)(v->x - v->y);
        v->y   = HALF_SQRT_2*(ftype)(v->x + v->y);
        v->x = tmp;
        return;
    }
    case ROTATION_YAW_90: {
        tmp = v->x; v->x = -v->y; v->y = tmp;
        return;
    }
    case ROTATION_YAW_135: {
        tmp = -HALF_SQRT_2*(ftype)(v->x + v->y);
        v->y   =  HALF_SQRT_2*(ftype)(v->x - v->y);
        v->x = tmp;
        return;
    }
    case ROTATION_YAW_180:
        v->x = -v->x; v->y = -v->y;
        return;
    case ROTATION_YAW_225: {
        tmp = HALF_SQRT_2*(ftype)(v->y - v->x);
        v->y   = -HALF_SQRT_2*(ftype)(v->x + v->y);
        v->x = tmp;
        return;
    }
    case ROTATION_YAW_270: {
        tmp = v->x; v->x = v->y; v->y = -tmp;
        return;
    }
    case ROTATION_YAW_315: {
        tmp = HALF_SQRT_2*(ftype)(v->x + v->y);
        v->y   = HALF_SQRT_2*(ftype)(v->y - v->x);
        v->x = tmp;
        return;
    }
    case ROTATION_ROLL_180: {
        v->y = -v->y; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_180_YAW_45: {
        tmp = HALF_SQRT_2*(ftype)(v->x + v->y);
        v->y   = HALF_SQRT_2*(ftype)(v->x - v->y);
        v->x = tmp; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_180_YAW_90:
    case ROTATION_PITCH_180_YAW_270: {
        tmp = v->x; v->x = v->y; v->y = tmp; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_180_YAW_135: {
        tmp = HALF_SQRT_2*(ftype)(v->y - v->x);
        v->y   = HALF_SQRT_2*(ftype)(v->y + v->x);
        v->x = tmp; v->z = -v->z;
        return;
    }
    case ROTATION_PITCH_180: {
        v->x = -v->x; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_180_YAW_225: {
        tmp = -HALF_SQRT_2*(ftype)(v->x + v->y);
        v->y   =  HALF_SQRT_2*(ftype)(v->y - v->x);
        v->x = tmp; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_180_YAW_270: 
    case ROTATION_PITCH_180_YAW_90: {
        tmp = v->x; v->x = -v->y; v->y = -tmp; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_180_YAW_315: {
        tmp =  HALF_SQRT_2*(ftype)(v->x - v->y);
        v->y   = -HALF_SQRT_2*(ftype)(v->x + v->y);
        v->x = tmp; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_90: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_45: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        tmp = HALF_SQRT_2*(ftype)(v->x - v->y);
        v->y   = HALF_SQRT_2*(ftype)(v->x + v->y);
        v->x = tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_90: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        tmp = v->x; v->x = -v->y; v->y = tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_135: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        tmp = -HALF_SQRT_2*(ftype)(v->x + v->y);
        v->y   =  HALF_SQRT_2*(ftype)(v->x - v->y);
        v->x = tmp;
        return;
    }
    case ROTATION_ROLL_270: {
        tmp = v->z; v->z = -v->y; v->y = tmp;
        return;
    }
    case ROTATION_ROLL_270_YAW_45: {
        tmp = v->z; v->z = -v->y; v->y = tmp;
        tmp = HALF_SQRT_2*(ftype)(v->x - v->y);
        v->y   = HALF_SQRT_2*(ftype)(v->x + v->y);
        v->x = tmp;
        return;
    }
    case ROTATION_ROLL_270_YAW_90: {
        tmp = v->z; v->z = -v->y; v->y = tmp;
        tmp = v->x; v->x = -v->y; v->y = tmp;
        return;
    }
    case ROTATION_ROLL_270_YAW_135: {
        tmp = v->z; v->z = -v->y; v->y = tmp;
        tmp = -HALF_SQRT_2*(ftype)(v->x + v->y);
        v->y   =  HALF_SQRT_2*(ftype)(v->x - v->y);
        v->x = tmp;
        return;
    }
    case ROTATION_PITCH_90: {
        tmp = v->z; v->z = -v->x; v->x = tmp;
        return;
    }
    case ROTATION_PITCH_270: {
        tmp = v->z; v->z = v->x; v->x = -tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_90: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        tmp = v->z; v->z = -v->x; v->x = tmp;
        return;
    }
    case ROTATION_ROLL_180_PITCH_90: {
        v->y = -v->y; v->z = -v->z;
        tmp = v->z; v->z = -v->x; v->x = tmp;
        return;
    }
    case ROTATION_ROLL_270_PITCH_90: {
        tmp = v->z; v->z = -v->y; v->y = tmp;
        tmp = v->z; v->z = -v->x; v->x = tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_180: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        v->x = -v->x; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_270_PITCH_180: {
        tmp = v->z; v->z = -v->y; v->y = tmp;
        v->x = -v->x; v->z = -v->z;
        return;
    }
    case ROTATION_ROLL_90_PITCH_270: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        tmp = v->z; v->z = v->x; v->x = -tmp;
        return;
    }
    case ROTATION_ROLL_180_PITCH_270: {
        v->y = -v->y; v->z = -v->z;
        tmp = v->z; v->z = v->x; v->x = -tmp;
        return;
    }
    case ROTATION_ROLL_270_PITCH_270: {
        tmp = v->z; v->z = -v->y; v->y = tmp;
        tmp = v->z; v->z = v->x; v->x = -tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_180_YAW_90: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        v->x = -v->x; v->z = -v->z;
        tmp = v->x; v->x = -v->y; v->y = tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_270: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        tmp = v->x; v->x = v->y; v->y = -tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_68_YAW_293: {
        float tmpx = v->x;
        float tmpy = v->y;
        float tmpz = v->z;
        v->x =  0.14303897231223747232853327204793 * tmpx +  0.36877648650320382639478111741482 * tmpy + -0.91844638134308709265241077446262 * tmpz;
        v->y = -0.33213277779664740485543461545603 * tmpx + -0.85628942146641884303193137384369 * tmpy + -0.39554550256296522325882847326284 * tmpz;
        v->z = -0.93232380121551217122544130688766 * tmpx +  0.36162457008209242248497616856184 * tmpy +  0.00000000000000002214311861220361 * tmpz;
        return;
    }
    case ROTATION_PITCH_315: {
        tmp  = HALF_SQRT_2*(ftype)(v->x - v->z);
        v->z = HALF_SQRT_2*(ftype)(v->x + v->z);
        v->x = tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_315: {
        tmp = v->z; v->z = v->y; v->y = -tmp;
        tmp = HALF_SQRT_2*(ftype)(v->x - v->z);
        v->z   = HALF_SQRT_2*(ftype)(v->x + v->z);
        v->x = tmp;
        return;
    }
    case ROTATION_PITCH_7: {
        const float sin_pitch = 0.1218693434051474899781908334262; // sinF(pitch);
        const float cos_pitch = 0.99254615164132198312785249072476; // cosF(pitch);
        float tmpx = v->x;
        float tmpz = v->z;
        v->x =  cos_pitch * tmpx + sin_pitch * tmpz;
        v->z = -sin_pitch * tmpx + cos_pitch * tmpz;
        return;
    }
    case ROTATION_ROLL_45: {
        tmp  = HALF_SQRT_2*(ftype)(v->y - v->z);
        v->z = HALF_SQRT_2*(ftype)(v->y + v->z);
        v->y = tmp;
        return;
    }
    case ROTATION_ROLL_315: {
        tmp  = HALF_SQRT_2*(ftype)(v->y + v->z);
        v->z = HALF_SQRT_2*(ftype)(v->z - v->y);
        v->y = tmp;
        return;
    }
    case ROTATION_CUSTOM: 
        // Error: caller must perform custom rotations via matrix multiplication
        //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        return;
    case ROTATION_MAX:
        break;
    }
    // rotation invalid
    //INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);
}

void vec3_rotate_inverse(Vector3f_t *v, enum RotationEnum rotation)
{
    Vector3f_t x_vec = {1.0f,0.0f,0.0f};
    Vector3f_t y_vec = {0.0f,1.0f,0.0f};
    Vector3f_t z_vec = {0.0f,0.0f,1.0f};

    vec3_rotate(&x_vec, rotation);
    vec3_rotate(&y_vec, rotation);
    vec3_rotate(&z_vec, rotation);

    Vector3f_t M0 = {x_vec.x, y_vec.x, z_vec.x};
    Vector3f_t M1 = {x_vec.y, y_vec.y, z_vec.y};
    Vector3f_t M2 = {x_vec.z, y_vec.z, z_vec.z};

    Vector3f_t v_vec;

    v_vec.x = M0.x * v->x + M1.x * v->y + M2.x * v->z;
    v_vec.y = M0.y * v->x + M1.y * v->y + M2.y * v->z;
    v_vec.z = M0.z * v->x + M1.z * v->y + M2.z * v->z;

    (*v) = v_vec;
}

bool vec3_is_nan(const Vector3f_t* v)
{
    return isnan(v->x) || isnan(v->y) || isnan(v->z);
}

bool vec3_is_inf(const Vector3f_t* v)
{
    return isinf(v->x) || isinf(v->y) || isinf(v->z);
}
/*------------------------------------test------------------------------------*/


